import socket
import json
import select


class receiver:
    def __init__(self, local_ip) -> None:
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.sock.settimeout(1)
        self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
        self.sock.bind((local_ip, 8001))
        pass

    def load(self, string):
        try:
            cmd = json.loads(string)
            fb = {}
            fb["fl_pos"] = cmd["fl_pos"]
            fb["fl_vel"] = cmd["fl_vel"]

            fb["fr_pos"] = cmd["fr_pos"]
            fb["fr_vel"] = cmd["fr_vel"]

            fb["bl_pos"] = cmd["bl_pos"]
            fb["bl_vel"] = cmd["bl_vel"]

            fb["br_pos"] = cmd["br_pos"]
            fb["br_vel"] = cmd["br_vel"]
            return fb
        except:
            print("exception on load")
            return None


idle = receiver("127.0.0.1")
odom = receiver("192.168.1.125")
while True:
    ready = select.select([idle.sock, odom.sock], [], [], 1)
    if len(ready[0]) == 0:
        print("timeout")
        continue
    for sock in ready[0]:
        string = sock.recv(1024)
        if sock == idle.sock:
            idle_fb = idle.load(string)
        elif sock == odom.sock:
            odom_fb = odom.load(string)
    try:
        print("Feedback Idle:")
        print(" fl_pos ", idle_fb["fl_pos"], " fl_vel ", idle_fb["fl_vel"],
              " fr_pos ", idle_fb["fr_pos"], " fr_vel ", idle_fb["fr_vel"])
        print(" bl_pos ", idle_fb["bl_pos"], " bl_vel ", idle_fb["bl_vel"],
              " br_pos ", idle_fb["br_pos"], " br_vel ", idle_fb["br_vel"])
        print("Feedback Real:")
        print(" fl_pos ", odom_fb["fl_pos"], " fl_vel ", odom_fb["fl_vel"],
              " fr_pos ", odom_fb["fr_pos"], " fr_vel ", odom_fb["fr_vel"])
        print(" bl_pos ", odom_fb["bl_pos"], " bl_vel ", odom_fb["bl_vel"],
              " br_pos ", odom_fb["br_pos"], " br_vel ", odom_fb["br_vel"])
        print("Feedback Error:")
        print(" fl_pos ", odom_fb["fl_pos"]-idle_fb["fl_pos"], " fl_vel ", odom_fb["fl_vel"]-idle_fb["fl_vel"],
              " fr_pos ", odom_fb["fr_pos"]-idle_fb["fr_pos"], " fr_vel ", odom_fb["fr_vel"]-idle_fb["fr_vel"])
        print(" bl_pos ", odom_fb["bl_pos"]-idle_fb["bl_pos"], " bl_vel ", odom_fb["bl_vel"]-idle_fb["bl_vel"],
              " br_pos ", odom_fb["br_pos"]-idle_fb["br_pos"], " br_vel ", odom_fb["br_vel"]-idle_fb["br_vel"])
        print("-----------------------------------------------------------------------------------")
        pass
    except:
        print("exception on print")
        pass
